#include "communication/communication_plugin.h"
#include <pluginlib/class_list_macros.h>
#include "smarteye_common/ParamCoord.h"


namespace commplugin {

class ParamCoordPlugin : public CommPlugin {
public:
        ParamCoordPlugin() :
                paramcoord_nh("~"),
                station_link(nullptr)
        { };

        void initialize(augmavconn::AugMavConnInterface::AugPtr station_link_, int target_system_id_, int target_component_id_)
        {
            station_link = station_link_;
            target_system_id = target_system_id_;
            target_component_id = target_component_id_;
                paramcoord_pub = paramcoord_nh.advertise<smarteye_common::ParamCoord>("paramcoord", 10, true);
        }

        const message_map get_rx_handlers() {
                return {
                               MESSAGE_HANDLER(MAVLINK_MSG_ID_PARAM_COOR, &ParamCoordPlugin::handle_param_coord)
                };
        }

private:
        ros::NodeHandle paramcoord_nh;
        augmavconn::AugMavConnInterface::AugPtr station_link;
        int target_system_id;
        int target_component_id;
        ros::Publisher paramcoord_pub;


        void handle_param_coord(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid)
        {
            mavlink_param_coor_t pcoorMsg;
            mavlink_msg_param_coor_decode(msg,&pcoorMsg);
            smarteye_common::ParamCoord paramcoord_rmsg;
            paramcoord_rmsg.Timestamp = pcoorMsg.Timestamp;
            paramcoord_rmsg.arc_length = pcoorMsg.arc_length / 1E2;
            paramcoord_rmsg.cur_wpt = pcoorMsg.cur_wpt;
            paramcoord_rmsg.ID = pcoorMsg.ID;
            paramcoord_pub.publish(paramcoord_rmsg);
        }

};


};


PLUGINLIB_EXPORT_CLASS(commplugin::ParamCoordPlugin, commplugin::CommPlugin)
